// Use the GNU Scientific Library and the algorith from the Ma book 'Theory and
// Applications of Antenna Arrays' to calculate for optimum directivity the
// amplitude and phase angles of element of an array.
//
// The following Octave script describes the algorithm:
// n = 2;
// linear = true;
// circle = false;
// wavelength = 537.471;
// # circle radius
// a = wavelength / 8;
// # linear spacing
// d = 70;
// #theta = pi / 2 + (0 / 180 * pi); # broadside
// theta = 0 + (0 / 180 * pi); # end-fire
// 
// B = zeros(n);
// 
// z = zeros(n,1);
// e = zeros(n,1);
// 
// k = 2 * pi / wavelength;
// 
// format free;
// 
// # linear array
// if (linear)
// for x = 1:n
//     z(x) = (x - 1) * d;
// endfor
// end
// 
// # circle array
// if (circle)
// for x= 1:n
//     phi(x) = 2 * pi * (x - 1) / n;
// endfor
// end
// 
// # calculate phase offset at each element
// # linear array
// if (linear)
// for x = 1:n
//     e(x) = exp(-j * k * z(x) * cos(theta));
// endfor
// end 
// 
// # circle array
// if (circle)
// for x = 1:n
//     e(x) = exp(-j * k * a * cos(theta - 2 * pi * (x - 1) / n));
// endfor
// end
// 
// # calculate sin(k * x)/(k * x) for each element pair where "x" is the distance
// # between the elements
// # linear array
// if (linear)
// for m = 1:n
//     for l = 1:n
//         if (l != m)
//             B(l,m) = sin(k * (z(m) - z(l))) / (k * (z(m) - z(l)));
//         else
//             B(l,m) = 1;
//         end
//     endfor
// endfor
// end
// 
// # circle array
// if (circle)
// for m = 1:n
//     for l = 1:n
//         rho = 2*a*sin((phi(m) - phi(l))/2);
//         if (l != m)
//             B(l,m) = sin(k * rho) / (k * rho);
//         else
//             B(l,m) = 1;
//         end
//     endfor
// endfor
// end
// 
// B_inverse = inverse(B);
// I_opt = B_inverse * e;
// 
// D_max = e' * B_inverse * e;
// 
// printf('Max Directivity is: %f\n', abs(D_max));
// 
// disp('Amplitudes');
// for x = 1:n
// #    disp(abs(I_opt(x)));
//     disp(abs(I_opt(x))/abs(I_opt(1)));
// endfor
// disp('Phases');
// for x = 1:n
//     if (abs(arg(I_opt(x)))>0.0000001)
//         disp((arg(I_opt(x)))/pi*180);
//     else
//         disp(0);
//     end
// #    disp((arg(I_opt(x))-arg(I_opt(1)))/pi*180);
// endfor

#include <complex.h>
#include <stdio.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_blas.h>
#include <gsl/gsl_complex.h>
#include <gsl/gsl_complex_math.h>
#include <gsl/gsl_linalg.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_permutation.h>
#include "optimize.h"
#include "gnuplot_i.h"

#define PI 3.1415926535897932384626434

// n:          number of elements in first dimension
// m:          number of elements in second dimension
// type:       either grid, linear, circle, or circle and center
// dimension1: 1st linear spacing or 1st circle radius
// dimension2: 2nd linear spacing or 2nd circle radius
// x:          result x coordinates
// y:          result y coordinates
int CreateArray(int n, int m, int type, double dimension1, double dimension2, double x[], double y[]) {
    double pi = M_PI;
    int linear = (type == ARRAY_TYPE_LINEAR);
    int grid = (type == ARRAY_TYPE_GRID);
    int circle = (type == ARRAY_TYPE_CIRCLE) || (type == ARRAY_TYPE_CIRCLE_CENTER);
    int center = (type == ARRAY_TYPE_CIRCLE_CENTER);
    double radius1 = 0, radius2 = 0, spacing = 0, stagger = 0;
    int i, j;

    // convert dimension1 to type specific dimension
    if (linear) {
        spacing = dimension1;
    } else if (grid) {
        spacing = dimension1;
        stagger = dimension2;
    } else if (circle) {
        radius1 = dimension1;
        radius2 = dimension2;
    }

    // linear array, 1 dimension only, n elements
    if (linear) {
        for (i = 0; i < n; i++) {
            double v;
            v = i * spacing;
            x[i] = 0;
            y[i] = v;
        }
        return 0;
    }

    // grid array, 2 dimensions, n * m elements
    if (grid) {
        for (j = 0; j < m; j++) {
            double w;
            w = j * stagger;
            for (i = 0; i < n; i++) {
                double v;
                v = i * spacing;
                x[j * n + i] = w;
                y[j * n + i] = v;
            }
        }
        return 0;
    }
  
    // circle array, n + m or n + m + 1 elements
    if (circle) {
        for (i = 0; i < n; i++) {
            double v;
            v = 2 * pi * i / n;
            x[i] = radius1 * cos(v);
            y[i] = radius1 * sin(v);
        }
        for (j = 0; j < m; j++) {
            double v;
            if (radius2 > 0) {
                v = 2 * pi * j / m;
            } else {
                radius2 = -radius2;
                v = 2 * pi * j / m - pi;
            }
            x[i + j] = radius2 * cos(v);
            y[i + j] = radius2 * sin(v);
        }
        if (center) {
            x[i + j + 1] = 0;
            y[i + j + 1] = 0;
        }
    }
    return 0;
}

// n:           number of elements
// x:           x coordinates
// y:           y coordinates
// wavelength:  wavelength in units the same as dimension1
// theta:       look angle in degrees: 90 means broadside, 0 means end-fire
// result:      complex element currents
// directivity: pointer to directivity result
int OptimizeArray(int n, double x[], double y[], double wavelength, double theta, complex result[], double *directivity) {
    double pi = M_PI;
    int i, l, m;

    theta *= pi / 180.0;

    gsl_matrix_complex *B = gsl_matrix_complex_alloc(n, n);
    gsl_matrix_complex *B_inverse = gsl_matrix_complex_alloc(n, n);

    gsl_matrix_complex *e = gsl_matrix_complex_alloc(n, 1);

    gsl_matrix_complex *I_opt = gsl_matrix_complex_alloc(n, 1);
    gsl_matrix_complex *temp = gsl_matrix_complex_alloc(1, n);
    gsl_matrix_complex *D_max = gsl_matrix_complex_alloc(1, 1);

    // calculate popular intermediate value
    double k = 2 * pi / wavelength;

    // calculate phase offset at each element and populate e matrix
    for (i = 0; i < n; i++) {
        double radius = hypot(x[i], y[i]);
        double angle = atan2(y[i], x[i]);
        gsl_complex v = gsl_complex_polar(1, -k * radius * cos(theta - angle));
            gsl_matrix_complex_set(e, i, 0, v);
        }

    // calculate sin(k * d)/(k * d) for each element pair where "d" is the
    // distance between the elements
    for (m = 0; m < n; m++) {
        for (l = 0; l < n; l++) {
            gsl_complex v;
            if (l != m) {
                double d = hypot(x[l] - x[m], y[l] - y[m]);
                GSL_SET_COMPLEX(&v, sin(k * d) / (k * d), 0);
                gsl_matrix_complex_set(B, l, m, v);
            } else {
                GSL_SET_COMPLEX(&v, 1, 0);
                gsl_matrix_complex_set(B, l, m, v);
            }
        }
    }

    int SIGNUM;
    gsl_permutation *p = gsl_permutation_alloc(n);
    gsl_linalg_complex_LU_decomp(B, p, &SIGNUM);
    gsl_linalg_complex_LU_invert(B, p, B_inverse);

    gsl_complex v, w;
    GSL_SET_COMPLEX(&v, 1.0, 0.0);
    GSL_SET_COMPLEX(&w, 0.0, 0.0);
    gsl_blas_zgemm(CblasNoTrans, CblasNoTrans, v, B_inverse, e, w, I_opt);

    gsl_blas_zgemm(CblasConjTrans, CblasNoTrans, v, e, B_inverse, w, temp);
    gsl_blas_zgemm(CblasNoTrans, CblasNoTrans, v, temp, e, w, D_max);

    *directivity = GSL_REAL(gsl_matrix_complex_get(D_max, 0, 0));

    for (i = 0; i < n; i++) {
        // convert GSL complex to C99 complex
        gsl_complex v = gsl_matrix_complex_get(I_opt, i, 0);
        result[i] = GSL_REAL(v) + I * GSL_IMAG(v);
    }

    gsl_matrix_complex_free(B);
    gsl_matrix_complex_free(B_inverse);

    gsl_matrix_complex_free(e);

    gsl_matrix_complex_free(I_opt);
    gsl_matrix_complex_free(temp);
    gsl_matrix_complex_free(D_max);

    return 0;
}

#ifdef TEST

#define ANGLE_INCREMENT 0.001
#define LOG_SCALE_LIMIT 100  // 10^(40 db range / 2) = 100

double directivity;
char *file_name;
char title[80];

int PlotArray(char *name, int type, int n, double wavelength, double x[], double y[], double C[], double A[]) {
    int i;
    struct {double r; double t;} polar[n];
    complex E;
    double theta, sin_last_theta, cos_last_theta, sin_theta = 0, cos_theta = 0;
    double k = 2 * PI / wavelength;

    // convert the rectangular coordinates to polar
    for (i = 0; i < n; i++) {
        polar[i].r = hypot(x[i], y[i]);
        polar[i].t = atan2(y[i], x[i]);
    }

    // set up gnuplot
    gnuplot_ctrl *handle;
    char comment[80];

    handle = gnuplot_init();
    if (type == PLOT_TYPE_IMAGE) {
        gnuplot_cmd(handle, "set terminal png size 1280, 1024");
        gnuplot_cmd(handle, "set output '%s'", file_name);
    } else if (type == PLOT_TYPE_SCREEN) {
    }
    gnuplot_cmd(handle, "set polar");
    gnuplot_cmd(handle, "set size square");
    gnuplot_cmd(handle, "set xrange[-1:1]");
    gnuplot_cmd(handle, "set yrange[-1:1]");
    gnuplot_cmd(handle, "set grid polar");
    gnuplot_cmd(handle, "set xtics (\"\" 0, \"-30\" 0.25, \"-20\" 0.5, \"-10\" 0.75, \"0\" 1)");
    gnuplot_cmd(handle, "set ytics (\"\" 0, \"-30\" 0.25, \"-20\" 0.5, \"-10\" 0.75, \"0\" 1)");
    gnuplot_cmd(handle, "set xtics axis");
    gnuplot_cmd(handle, "set ytics axis");
    gnuplot_cmd(handle, "set title '%s'", title);
    gnuplot_cmd(handle, "set key outside top");
    gnuplot_cmd(handle, "set key title 'legend'");
    gnuplot_cmd(handle, "set key autotitle columnheader");

    // sum the array
    // E(theta) = f(theta)*Ii*exp[j*(k*ri*cos(theta-ti)+Ai)]

    // where theta is an arbitrary angle, f is the element pattern, Ii is an
    // element current amplitude, k is 2*pi/wavelength, ri and ti is element
    // position in polar form, and Ai is element current phase.

    // Since we are doing this in two dimensions, and the elements are
    // omni-directional, then f(theta) is 1.

    int beam_width_found = 0;
    double e, last_e = 0, average = 0, maximum = 0;
    double beam_center = 0, beam_width = 0;
    double sin_increment = sin(ANGLE_INCREMENT);
    double cos_increment = cos(ANGLE_INCREMENT);

    // iterate a circle to determine the maximum and average values
    for (theta = 0.0; theta < 2.0 * PI; theta += ANGLE_INCREMENT) {
        // compute sin(theta) and cos(theta)
        if (theta == 0.0) {
            sin_last_theta = 0.0;
            cos_last_theta = 1.0;
            sin_theta = 0.0;
            cos_theta = 1.0;
        } else {
            sin_last_theta = sin_theta;
            cos_last_theta = cos_theta;
            sin_theta = sin_increment * cos_last_theta + cos_increment * sin_last_theta;
            cos_theta = cos_increment * cos_last_theta - sin_increment * sin_last_theta;
        }
        // iterate the array elements
        E = 0;
        for (i = 0; i < n; i++) {
            double cos_diff = cos_theta * cos(polar[i].t) + sin_theta * sin(polar[i].t);
            E += C[i] * cexp(I * (-k * polar[i].r * cos_diff + A[i]));
        }
        // track the average magnitude and the maximum value
        e = cabs(E);
        average += e / (2 * PI / ANGLE_INCREMENT);
        if (e > maximum) {
            maximum = e;
            beam_center = theta;
            beam_width_found = 0;
        }
        if (!beam_width_found && (e < last_e) && (e < maximum * sqrt(2) / 2)) {
            beam_width = (theta - beam_center) * 2.0;
            beam_width_found = 1;
        }
        last_e = e;
    }

    // calculate efficiency = max / (n * sum(C[i]^2))
    double currents_squared = 0;
    for (i = 0; i < n; i++) {
        currents_squared += C[i] * C[i];
    }
    double efficiency = maximum * maximum / (n * currents_squared);
    
    double text_height = 0.08;
    char *font = "'courier'";
    if (type == PLOT_TYPE_IMAGE) {
        font = "'VeraMoBd,8'";
        text_height = 0.03;
    }

    gnuplot_cmd(handle, "set lmargin 20");
    gnuplot_cmd(handle, "set label ' # ---x--- ---y---' at -1.05, 0.95 right font %s", font);
    for (i = 0; i < n; i++) {
        snprintf(comment, sizeof(comment), "%2d %7.2f %7.2f", i + 1, x[i], y[i]);
        gnuplot_cmd(handle, "set label '%s' at -1.05, %f right font %s", comment, 0.95 - text_height * (i + 1), font);
    }
    gnuplot_cmd(handle, "set label ' # Amplitude Phase ' at -1.05, -0.05 right font %s", font);
    for (i = 0; i < n; i++) {
        snprintf(comment, sizeof(comment), "%2d %9.4f %6.1f", i + 1, C[i] / C[0], A[i] / PI * 180);
        gnuplot_cmd(handle, "set label '%s' at -1.05, %f right font %s", comment, -0.05 - text_height * (i + 1), font);
    }

    gnuplot_cmd(handle, "set rmargin 20");
    snprintf(comment, sizeof(comment), "directivity = %5.3f", directivity);
    gnuplot_cmd(handle, "set label '%s' at 1.05, -0.63", comment);
    snprintf(comment, sizeof(comment), "beam width = %.2f", beam_width * 180 / PI);
    gnuplot_cmd(handle, "set label '%s' at 1.05, -0.71", comment);
    snprintf(comment, sizeof(comment), "efficiency = %f", efficiency);
    gnuplot_cmd(handle, "set label '%s' at 1.05, -0.79", comment);
    snprintf(comment, sizeof(comment), "average = %f", average);
    gnuplot_cmd(handle, "set label '%s' at 1.05, -0.87", comment);
    snprintf(comment, sizeof(comment), "maximum = %f", maximum);
    gnuplot_cmd(handle, "set label '%s' at 1.05, -0.95", comment);
    gnuplot_cmd(handle, "plot '-' with lines");
    gnuplot_cmd(handle, "radians %s", name);

    // iterate a circle to plot
    for (theta = 0.0; theta < 2.0 * PI; theta += ANGLE_INCREMENT) {
        // compute sin(theta) and cos(theta)
        if (theta == 0.0) {
            sin_last_theta = 0.0;
            cos_last_theta = 1.0;
            sin_theta = 0.0;
            cos_theta = 1.0;
        } else {
            sin_last_theta = sin_theta;
            cos_last_theta = cos_theta;
            sin_theta = sin_increment * cos_last_theta + cos_increment * sin_last_theta;
            cos_theta = cos_increment * cos_last_theta - sin_increment * sin_last_theta;
        }
        // iterate the array elements
        E = 0;
        for (i = 0; i < n; i++) {
            double cos_diff = cos_theta * cos(polar[i].t) + sin_theta * sin(polar[i].t);
            E += C[i] * cexp(I * (-k * polar[i].r * cos_diff + A[i]));
        }
        // print the magnitude of E in db
        if (log10(cabs(E) * LOG_SCALE_LIMIT / maximum) > 0)
            gnuplot_cmd(handle, "%f %f", theta, log10(cabs(E) * LOG_SCALE_LIMIT / maximum) / log10(LOG_SCALE_LIMIT));
    }

    gnuplot_cmd(handle, "e");
    if (type == PLOT_TYPE_SCREEN) {
        struct timeval timeout = {0};
        fd_set ready;
        do {
            FD_ZERO(&ready);
            FD_SET(STDIN_FILENO, &ready);
            select(STDIN_FILENO + 1, &ready, NULL, NULL, &timeout);
            usleep(100000);
        } while (!FD_ISSET(STDIN_FILENO, &ready));
    }
    gnuplot_close(handle);

    return 0;
}

#define TYPE_LINEAR 1
#define TYPE_GRID 2
#define TYPE_CIRCLE 3
#define TYPE_CIRCLE_CENTER 4
#define TYPE_LINEAR_STR "linear"
#define TYPE_GRID_STR "grid"
#define TYPE_CIRCLE_STR "circle"
#define TYPE_CIRCLE_CENTER_STR "circle-center"

int main(int argc, char *argv[]) {
    int i;
    double pi = M_PI;
    double wavelength = 530.226990;
    double dimension1 = 290.0;
    double dimension2 = 70.0;
    int plot_type = PLOT_TYPE_SCREEN;
    int n = 4;
    int m = 2;
    double steering = 70.0;
    double layout_bias = 67.0;
    int array_type =  TYPE_GRID, local_array_type = 0;
    char name[20];
    int elements = 0;
    int opt;
    double theta = 0;
    char elements_str[80];
    int no_plot = 0;

    while ((opt = getopt(argc, argv, "a:b:1:2:s:n:v:n:m:w:i:x")) != -1) {
        switch (opt) {
        case 'a':
            if (strcmp(optarg, TYPE_LINEAR_STR) == 0) {
                array_type = TYPE_LINEAR;
            } else if (strcmp(optarg, TYPE_GRID_STR) == 0) {
                array_type = TYPE_GRID;
            } else if (strcmp(optarg, TYPE_CIRCLE_STR) == 0) {
                array_type = TYPE_CIRCLE;
            } else if (strcmp(optarg, TYPE_CIRCLE_CENTER_STR) == 0) {
                array_type = TYPE_CIRCLE_CENTER;
            } else {
                fprintf(stderr, "Invalid element type: \"%s\"\n", optarg);
            }
            break;
        case 'b':
            layout_bias = atof(optarg);
            break;
        case '1':
            dimension1 = atof(optarg);
            break;
        case '2':
            dimension2 = atof(optarg);
            break;
        case 's':
            steering = atof(optarg);
            break;
        case 'h':
            /* Force help to be shown */
            break;
        case 'n':
            n = atoi(optarg);
            break;
        case 'm':
            m = atoi(optarg);
            break;
        case 'w':
            wavelength = atof(optarg);
            break;
        case 'i':
            file_name = strdup(optarg);
            plot_type = PLOT_TYPE_IMAGE;
            break;
        case 'x':
            no_plot = 1;
            break;
        default:
            fprintf(stderr, "missing argument for %c\n", opt);
            break;
        }
    }
    
    if (abs(dimension2) > abs(dimension1)) {
        double temp = dimension2;
        dimension1 = dimension2;
        dimension2 = temp;
    }

    if (array_type == TYPE_LINEAR) {
        local_array_type = ARRAY_TYPE_LINEAR;
        theta = -90.0;
        elements = n;
    } else if (array_type == TYPE_GRID) {
        local_array_type = ARRAY_TYPE_GRID;
        theta = -90.0;
        elements = n * m;
    } else if (array_type == TYPE_CIRCLE) {
        local_array_type = ARRAY_TYPE_CIRCLE;
        theta = 0.0;
        elements = n + m;
    } else if (array_type == TYPE_CIRCLE_CENTER) {
        local_array_type = ARRAY_TYPE_CIRCLE_CENTER;
        theta = 0.0;
        elements = n + m + 1;
    }
    // apply array dependent physical versus radiational direction bias
    steering += theta;
    // convert geographical angle to mathematical angle
    steering = 90.0 - steering;

    double x[elements];
    double y[elements];

    CreateArray(n, m, local_array_type, dimension1, dimension2, x, y);

    if (!no_plot) {
        printf("    ---x---   ---y---\n");
        for (i = 0; i < elements; i++) {
            printf("%2d  %7.2f   %7.2f\n", i + 1, x[i], y[i]);
        }
    }
    
    complex result[elements];

    OptimizeArray(elements, x, y, wavelength, steering, result, &directivity);

    printf("Max Directivity is: %0.2f\n", directivity);

    if (!no_plot) {
        printf("Amplitudes   Phases\n");
        for (i = 0; i < elements; i++) {
            printf("%2d  %6.3f   %6.1f\n", i + 1,
                   cabs(result[i]) / cabs(result[0]), carg(result[i]) / pi * 180);
        }
    }
    
    double C[elements];
    double A[elements];

    for (i = 0; i < elements; i++) {
        C[i] = cabs(result[i]) / cabs(result[0]);
        A[i] = carg(result[i]);
    }

    if (no_plot)
        return 0;

    char *dimension1_str = 0, *dimension2_str = 0;
    if (array_type == TYPE_LINEAR) {
        dimension1_str = "Spacing";
        dimension2_str = 0;
        snprintf(elements_str, sizeof(elements_str), "%d Element Linear", n);
    } else if (array_type == TYPE_GRID) {
        dimension1_str = "Spacing";
        dimension2_str = "Stagger";
        snprintf(elements_str, sizeof(elements_str), "%d by %d Element Grid", n, m);
    } else if (array_type == TYPE_CIRCLE) {
        if (abs(dimension2) > 0) {
            snprintf(elements_str, sizeof(elements_str), "%d + %d Element Circle", n, m);
            dimension1_str = "Outer Radius";
            dimension2_str = "Inner Radius";
        } else {
            snprintf(elements_str, sizeof(elements_str), "%d Element Circle", n);
            dimension1_str = "Radius";
            dimension2_str = 0;
        }
    } else if (array_type == TYPE_CIRCLE_CENTER) {
        if (abs(dimension2) > 0) {
            snprintf(elements_str, sizeof(elements_str), "%d + %d Element Circle + Center", n, m);
            dimension1_str = "Outer Radius";
            dimension2_str = "Inner Radius";
        } else {
            snprintf(elements_str, sizeof(elements_str), "%d Element Circle + Center", n);
            dimension1_str = "Radius";
            dimension2_str = 0;
        }
    }

    if (dimension2_str) {
        snprintf(title, sizeof(title), "%s Array, %s %.2f, %s %.2f",
                 elements_str, dimension1_str, dimension1, dimension2_str, dimension2);
    } else {
        snprintf(title, sizeof(title), "%s Array, %s %.2f",
                 elements_str, dimension1_str, dimension1);
    }

    snprintf(name, sizeof(name), "%0.1f deg", steering);
    PlotArray(name, plot_type, elements, wavelength, x, y, C, A);

    return 0;
}
#endif
